﻿using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO;

namespace AGage.PolarExplorer
{

    /// <summary>
    /// Data provider interface for boat performance data.
    /// Different data formats may be implemented by adding classes 
    /// implementing this interface.
    /// </summary>
    public interface PerformanceDataProvider
    {
        /// <summary>
        /// The TWA values the boatspeed data is based upon.
		/// These values MUST be expressed in DEGREES.
        /// </summary>
        double[] WindAngles { get; }

        /// <summary>
        /// The TWS (values the boatspeed data is based upon.
		/// These values MUST be expressed in m/s METRES PER SECOND
        /// </summary>
        double[] WindStrengths { get; }

        /// <summary>
        /// A matrix of values for boatspeed: [twa,tws]
		/// These values MUST be expressed in KNOTS.
        /// Example: if WindAngle[9] = 35 degrees, and WindSpeed[10] = 5 m/s (~10knots) ->
        /// BoatSpeeds[9,10] returns the boat speed at TWA = 35 in 5 m/s of wind.
        /// </summary>
        double[,] BoatSpeeds { get; }
    }
    
	///
	/// This class encapsulates the performance data of a boat. It is called 'polar'
	/// as typically this data is represented visually with a graph in polar coordinates.
	/// There is no expected input format, other than that specified in the 
	/// PerformanceDataProvider interface, an instance of which must be passed to the
	/// constructor upon instantiation. Bi-Cubis interpolation is used to compute
	/// values at arbitrary intervals within the bounds of the input data, although some
	/// extrapolation is possible close to the upper bound of input TWS (obviously -ve TWS
	/// is meaningless)
    ///
    /// This class is an streamlined version of that found in the SOL Core dll for use in this application!
    public class PerformancePolar
    {
        private const int MIN_DP = 90 * 30; // Min data points to use SOL interp.
        private readonly alglib.spline2dinterpolant _interp;
        public static readonly double KNT_MS = 1.94384449;
        public static readonly double TO_RAD = Math.PI / 180;
        public readonly PerformanceDataProvider dataProvider;
        double[] tws, twa;
        double[,] bs;
        int dataPoints;

        public PerformancePolar(PerformanceDataProvider data)
        {
            dataProvider = data;

            // Extract data from the provider
            tws = data.WindStrengths;
            twa = data.WindAngles;
            bs = data.BoatSpeeds;
            dataPoints = tws.Length * twa.Length;

            // Initialize interpolant
            alglib.spline2dbuildbicubic(tws, twa, data.BoatSpeeds, twa.Length, tws.Length, out _interp);
        }
                
        /// <summary>
        /// Return the boatspeed for the corresponding twa (0-180) and tws (knots).
        /// The result is interpolated by 2D bicubic interpolation.
        /// The absolute value of twa is used to allow for -ve values to indicate
        /// opposite tack/gybe.
        /// </summary>
        /// <param name="twa"></param>
        /// <param name="tws"></param>
        /// <returns></returns>
        public double this[double twa, double tws]
        {
            get
            {                
				while (twa < 0) twa += 360;
				while (twa >= 360) twa -= 360;
				if (twa > 180) twa = 360 - twa; // turn 190 into (-)170
                
                // More 'SOL Accurate' if data source is a csv file:
                if (dataProvider is CSVPolarProvider || dataPoints >= 3000)
                    return Interp.Interpolate(this.tws, this.twa, this.bs, tws / KNT_MS, twa);
                else
                    return alglib.spline2dcalc(_interp, tws / KNT_MS, twa);
            }
        }

        /// <summary>
        /// Return an object with the details for maximum upwind VMG in the specified TWS.
        /// As this is VMG, velocity.heading == twa
        /// </summary>
        /// <param name="tws"></param>
        /// <returns></returns>
        public VelocityMadeGood MaxUpwindVMG(double tws)
        {
            double max_vmg_spd = -1, max_vmg_hdg = -1, max_bs = -1;
            for (double hdg = 20; hdg < 90; hdg += 0.25)
            {
                double bs = this[hdg, tws];
                double vmg = Math.Cos(hdg * TO_RAD) * bs;
                if (vmg > max_vmg_spd)
                {
                    max_vmg_spd = vmg;
                    max_vmg_hdg = hdg;
                    max_bs = bs;
                }
            }
            return new VelocityMadeGood(new Velocity(max_bs, max_vmg_hdg), max_vmg_spd, max_vmg_hdg, 0);
        }

        /// <summary>
        /// Return the TWA for maximising Downwind VMG in the specified TWS.
        /// </summary>
        /// <param name="tws"></param>
        /// <returns></returns>
        public VelocityMadeGood MaxDownwindVMG(double tws) //double max_vmg_spd, out double max_vmg_hdg)
        {
            double max_vmg_spd = -1, max_vmg_hdg = -1, max_bs = -1;
            for (double hdg = 90; hdg <= 180; hdg += 0.25)
            {
                double bs = this[hdg, tws];
                double vmg = Math.Abs(Math.Cos(hdg * TO_RAD)) * bs;
                if (vmg > max_vmg_spd)
                {
                    max_vmg_spd = vmg;
                    max_vmg_hdg = hdg;
                    max_bs = bs;
                }
            }
            return new VelocityMadeGood(new Velocity(max_bs, max_vmg_hdg), max_vmg_spd, max_vmg_hdg, 180);
        }

        public Velocity MaximumBoatSpeed(double tws)
        {
            double max_spd = -1, max_hdg = -1;
            for (double hdg = 0; hdg <= 180; hdg += 0.25)
            {
                double bs = this[hdg, tws];
                if (bs > max_spd)
                {
                    max_spd = bs;
                    max_hdg = hdg;
                }
            }

            return new Velocity(max_spd, max_hdg);
        }
        /// <summary>
        /// Calculate heading which acheives the greatest VMC with repsect to a waypoint.
        /// 1st element returned is port tack vmc
        /// </summary>
        /// <param name="tws"></param>
        /// <param name="twd"></param>
        /// <param name="cc">The Compass course (degrees) from currrent location to the Waypoint</param>
        /// <returns>The heading to sail at to acheive max VMC. 1st element is for port tack/gybe</returns>
        public VelocityMadeGood MaxVMC(double tws, double twd, double cc)
        {

            double vmc = -1, hdg = -1, bs = -1, twa = -1;

            // PORT: +ve twa
            for (double _hdg = twd; _hdg < twd + 180; _hdg += 0.25)
            {
                double _twa = _hdg - twd; //Math.Abs(twd - _hdg);
                double _bs = this[_twa, tws];
                double _vmc = Math.Cos((_hdg - cc) * TO_RAD) * _bs;

                if (_vmc > vmc)
                {
                    vmc = _vmc;
                    hdg = _hdg;
                    bs = _bs;
                    twa = _twa;
                }
            }
            if (hdg < 0) hdg += 360;
            if (hdg >= 360) hdg -= 360;

            return new VelocityMadeGood(new Velocity(bs, hdg), vmc, twa, cc);
        }
    }

    /// <summary>
    /// Convenience class to ease implementation of boat performance providers.
    /// Simply subclass and implement InitData(string).
    /// </summary>
    public abstract class AbstractPerformanceDataProvider : PerformanceDataProvider
    {
        protected double[] twa, tws;
        protected double[,] bs;

        public AbstractPerformanceDataProvider(string param)
        {
            if (param != null)
            {
                InitData(param);
                if (bs == null)
                    throw new Exception("Performance Data not initialised. param = " + param);
            }
        }

        /// <summary>
        /// Invoked upon conclassion to actually do the work of reading the data
        /// from whatever specific format the provider is for.
        /// </summary>
        /// <param name="param"></param>
        protected abstract void InitData(string param);

        public double[] WindAngles { get { return twa; } }

        /// <summary>
        /// The TWS values the boatspeed data is based upon in M/S.
        /// </summary>
        public double[] WindStrengths { get { return tws; } }

        /// <summary>
        /// A matrix of values for boatspeed: [twa,tws].
        /// Example: if WindAngle[9] = 35 degrees, and WindSpeed[10] = 15 knots ->
        /// BoatSpeeds[9,10] = speed at TWA=35 in 15 knots of wind.
        /// </summary>
        public double[,] BoatSpeeds { get { return bs; } }
    }

    /// <summary>
    /// A PerformanceDataProvider which reads performance information from
    /// 'comma seperated values' file.
    /// </summary>
    public class CSVPolarProvider : AbstractPerformanceDataProvider
    {
        public CSVPolarProvider(string file_name) : base(file_name)
        {   }        

        /// <summary>
        /// Generate the data provider from a string containing all the data instread of a file name
        /// </summary>
        /// <param name="_data"></param>
        /// <returns></returns>
        public static CSVPolarProvider FromStringData(string _data)
        {
            CSVPolarProvider rval = new CSVPolarProvider(null);
            List<double> _tws = new List<double>(), _twa = new List<double>();
            Dictionary<IntWindVector, double> bs_data = new Dictionary<IntWindVector, double>();

            foreach (string line in _data.Split('\n'))
            {
                // Ignore lines that do not begin with a number
                if (line.Length == 0 || !Char.IsDigit(line[0]))
                    continue;

                // comma separate, tokenize...
                // TWS(m/s), TWS(knots), TWA, SPD(knots)
                // we use tws in m/s because they are integral, as is twa
                string[] data = line.Split(',', ';', '\t'); // Pasted CSV from Excel etc actually uses tabs!

                IntWindVector wind = new IntWindVector(Int32.Parse(data[0], CultureInfo.InvariantCulture), Int32.Parse(data[2], CultureInfo.InvariantCulture));
                if (!_tws.Contains(wind.tws))
                    _tws.Add(wind.tws);
                if (!_twa.Contains(wind.twa))
                    _twa.Add(wind.twa);
                bs_data.Add(wind, double.Parse(data[3], CultureInfo.InvariantCulture));
            }

            rval.twa = _twa.ToArray();
            rval.tws = _tws.ToArray();
            rval.bs = new double[rval.twa.Length, rval.tws.Length];
            foreach (IntWindVector wv in bs_data.Keys)
                rval.bs[wv.twa, wv.tws] = bs_data[wv];

            return rval;
        }

        protected override void InitData(string csv_filename)
        {
            // Load data from csv file            
            StreamReader input = File.OpenText(csv_filename);
            string line;

            List<double> _tws = new List<double>(), _twa = new List<double>();
            Dictionary<IntWindVector, double> bs_data = new Dictionary<IntWindVector, double>();

            while (!input.EndOfStream)
            {
                line = input.ReadLine().Trim();

                // Ignore lines that do not begin with a number
                if (!Char.IsDigit(line[0]))
                    continue;

                // comma separate, tokenize...
                // TWS(m/s), TWS(knots), TWA, SPD(knots)
                // we use tws in m/s because they are integral, as is twa
                string[] data = line.Split(',', ';');

                IntWindVector wind = new IntWindVector(Int32.Parse(data[0], CultureInfo.InvariantCulture), Int32.Parse(data[2], CultureInfo.InvariantCulture));
                if (!_tws.Contains(wind.tws))
                    _tws.Add(wind.tws);
                if (!_twa.Contains(wind.twa))
                    _twa.Add(wind.twa);
                bs_data.Add(wind, double.Parse(data[3], CultureInfo.InvariantCulture));
            }

            twa = _twa.ToArray();
            tws = _tws.ToArray();
            bs = new double[twa.Length, tws.Length];
            foreach (IntWindVector wv in bs_data.Keys)
                bs[wv.twa, wv.tws] = bs_data[wv];
        }

        class IntWindVector
        {
            public int tws, twa;
            public IntWindVector(int _ws, int _wa)
            {
                tws = _ws; twa = _wa;
            }
        }
    }

    /// <summary>
    /// Parses its data from a sailonline style text file:
    /// [BOAT CLSS]
    /// TWA/TWS tab sep TWS
    /// tab sep TWA and BS
    /// </summary>
    public class TextPolarProvider : AbstractPerformanceDataProvider
    {
        //protected double[] twa, tws;
        //protected double[,] bs;

        public TextPolarProvider(string input)
            : base(input)
        { }

        protected override void InitData(string input)
        {
            List<double> twaL = new List<double>(),
                twsL = new List<double>();
            List<List<double>> bsL = new List<List<double>>();
            List<double> bs0 = new List<double>();
            bool needZero = false;
            bool haveZeroTWA = false;

            string[] lines = input.Split('\n');
            string[] col_vals;
            int n = 0; // skip first line containing boat class - preincrement in while

            while (++n < lines.Length)
            {
                if (lines[n].Length == 0)
                    continue;

                col_vals = lines[n].Trim().Split('\t', ' ');
                if (col_vals.Length < 2)
                    continue;
                if (n == 1)
                {// get TWSs
                    double k = 1;
                    if (parse(col_vals[col_vals.Length - 1]) > 20)
                        k = PerformancePolar.KNT_MS;

                    int i = 1;
                    while (col_vals[i].Length == 0)
                        ++i; // some polars with space delimiting result in empty elements
                    if (parse(col_vals[i]) != 0)
                    {
                        twsL.Add(0); // add a 0 TWS for better interpolation of values < whatever min TWS is psecified in the polar
                        needZero = true;
                    }
                    for (i = 1; i < col_vals.Length; ++i)
                    {
                        if (col_vals[i].Length > 0)
                            twsL.Add(parse(col_vals[i]) / k);
                    }
                    continue;
                }

                if (!haveZeroTWA)
                {
                    double firstTWA = parse(col_vals[0]);
                    if (firstTWA > 0)
                    {
                        twaL.Add(0);
                        List<double> __bs = new List<double>();
                        for (int i = 0; i < twsL.Count; ++i)
                            __bs.Add(0);
                        bsL.Add(__bs);
                    }
                    haveZeroTWA = true;
                }

                // 1st value is TWA, then list of BS:
                List<double> _bs = new List<double>();
                twaL.Add(parse(col_vals[0]));
                if (needZero)
                    _bs.Add(0); // 0 BS to go with the 0 TWS...
                for (int i = 1; i < col_vals.Length; ++i)
                {
                    if (col_vals[i].Length > 0)
                        _bs.Add(parse(col_vals[i]));
                }
                bsL.Add(_bs);
            }

            twa = twaL.ToArray();
            tws = twsL.ToArray();
            bs = new double[twaL.Count, twsL.Count];
            for (int i = 0; i < twaL.Count; ++i)
                for (int j = 0; j < twsL.Count; ++j)
                {
                    double __bs = bsL[i][j];
                    bs[i, j] = __bs;
                }
        }

        double parse(string val)
        {
            return double.Parse(val, System.Globalization.CultureInfo.InvariantCulture);
        }
    }

    /* 
     * Interpolate as on sailonline.org:
     */
    public class Interp
    {
        public static double Interpolate(double[] X, double[] Y, double[,] Z, double x, double y)
        {
            int xi, yi;
			
			// Binary search the indices:
			int N        = X.Length - 1;
			double min_x =     x > X[0] ? x     : X[0];
			double  _x   = min_x < X[N] ? min_x : X[N];
			int lo       = 0;
			int hi       = N;
			int mid      = (int)((lo + hi) / 2); // Call to Math.round not required and slow inside loop...

			while ( (hi - lo) >= 3 )
			{
				if ( _x < X[mid] )
					hi = mid;
				else
					lo = mid;
				mid = (int)((lo + hi) / 2);
			}
			if ( _x < X[mid] )
				xi = lo;
			else
				xi = mid;			
			
			    N        = Y.Length - 1;
			double y_min =     y > Y[0] ? y     : Y[0];
			double  _y   = y_min < Y[N] ? y_min : Y[N];
			lo                 = 0;
			hi                 = N;
			mid                = (int)((lo + hi) / 2);
			
			while ( (hi - lo) >= 3 )
			{
				if ( _y < Y[mid] )
					hi = mid;
				else
					lo = mid;
                mid = (int)((lo + hi) / 2);
			}
			if ( _y < Y[mid] )
				yi = lo;
			else
				yi = mid;		
			
			// Compute the interpolated value and return:
			
			return (
				((Z[yi+1,xi]-Z[yi+1,xi+1])/(X[xi]-X[xi+1])*(x-X[xi])+Z[yi+1,xi]-(Z[yi,xi]-Z[yi,xi+1])/(X[xi]-X[xi+1])*(x-X[xi])-Z[yi,xi])
				     /
				(Y[yi+1]-Y[yi])*(y-Y[yi+1])+(Z[yi+1,xi]-Z[yi+1,xi+1])/(X[xi]-X[xi+1])*(x-X[xi])+Z[yi+1,xi]
			     );
        }
    }

    public class Velocity
    {
        public double speed, heading;
        public Velocity(double spd, double hdg)
        {
            speed = spd; heading = hdg;
        }

        public static Velocity operator *(Velocity lhs, double rhs)
        {
            return new Velocity(lhs.speed * rhs, lhs.heading);
        }

    }

    /// <summary>
    /// Represents the Velocity with respect to a direction other than the heading of the actual
    /// object's velocity. If the angle between heading and COURSE is @, then vmc = cos(@) * speed.
    /// for completeness the twa is also maintained in this classure.
    /// </summary>
    public class VelocityMadeGood
    {
        /// <summary>
        /// The boat's velocity and heading.
        /// </summary>
        public Velocity velocity;
        /// <summary>
        /// The vmc value.
        /// </summary>
        public double vmg;
        /// <summary>
        /// The TWA at which the boat is sailing.
        /// </summary>
        public double twa;
        /// <summary>
        /// The compass course to which this is the VMC value.
        /// This field is not always set, depending on the source of the object.
        /// For example, MaxVMG methods from PerformancePolar class, return an object of this class,
        /// and set this value to an offset for the TWD (ie upwind = 0, downwind = 180).
        /// </summary>
        public double cc;

        public VelocityMadeGood(Velocity v, double _vmg, double _twa, double _cc)
        {
            velocity = v; vmg = _vmg; twa = _twa; cc = _cc;
        }

        public VelocityMadeGood(double spd, double hdg, double _vmg, double _twa, double _cc)
            : this(new Velocity(spd, hdg), _vmg, _twa, _cc)
        {}
    }
}
